package com.rockwell.sniffyhunter.model;

import com.rockwell.sniffyhunter.utils.IStateMachine;
import com.rockwell.sniffyhunter.utils.MoveBackToSniffState;

public class MoveBackToSniff extends StateObject {
	private double m_distanceToMoveBack;
	private double m_angleToTurnBack;
	private RobotCommandSequencer m_commandSequencer;

	public MoveBackToSniff(Robot robot) {
		super(robot);
		m_commandSequencer = new RobotCommandSequencer(robot);
	}
	
	@Override
	public void Initialize() {
		//m_commandSequencer.Initialize();
		machineState = MoveBackToSniffState.START;
		m_angleToTurnBack = 0;
		m_distanceToMoveBack = 0;
		
	}

	@Override
	public boolean IsDone() {
		return machineState == MoveBackToSniffState.DONE;
	}

	public ReturnResult PerformAction(double distanceToMoveBack, double angleToTurnBack)  {
		if (machineState == MoveBackToSniffState.START) {
			m_distanceToMoveBack = -distanceToMoveBack;
			m_angleToTurnBack = - angleToTurnBack;
		}

		return PerformAction();
	}

	public IStateMachine moveBack() {
		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.Move, m_distanceToMoveBack);
		
		if (returnVal == ReturnResult.Completed) {
			log("moveBack.MoveBackToSniff", 4);
			
			if (m_angleToTurnBack < 2) {
				machineState = MoveBackToSniffState.DONE;
			}
			else {
				machineState = MoveBackToSniffState.TURN_BACK;
			}
		}
		
		return machineState;
	}
	
	public IStateMachine turnBack() {
		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.TurnDegree, m_angleToTurnBack);
		
		if (returnVal == ReturnResult.Completed) {
			log("turnBack.MoveBackToSniff", 4);
			machineState = MoveBackToSniffState.DONE;
		}
		
		return machineState;
	}
}